//------------------------------------------------------------------------------
//
//  TeleOperation Service
//
//  Written Jan 2008 by Trevor Taylor, Software Technology, Australia
//
// Updated for July CTP of MRDS V2.0
//
// NOTE:
// For testing purposes, the Project Properties have the following
// appended to the command line arguments in the Debug tab: 
// -manifest:"ProMRDS/Config/MazeSimulator.manifest.xml"
// If you are rebuilding these samples chapter by chapter then you
// might not have built the MazeSimulator yet.
// If you want to test the TeleOperation service with a real robot,
// change the manifest to run your robot instead of the Maze Simulator.
//
// When running on a 64-bit operating system you will need to change the
// Start external program in the Debug tab to run DssHost32.exe instead
// of DssHost.exe otherwise the simulator will not run.
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using W3C.Soap;
using teleoperation = ProMRDS.Robotics.TeleOperation;

using mount = Microsoft.Dss.Services.MountService;

// Added references
// For Forms
using Microsoft.Ccr.Adapters.WinForms;
using System.Windows.Forms;
// For handling webcam images
using System.Drawing;
using System.Drawing.Imaging;
using System.Runtime.InteropServices;
// Game Controller
using game = Microsoft.Robotics.Services.GameController.Proxy;
// Generic Differential Drive
using drive = Microsoft.Robotics.Services.Drive.Proxy;
// Webcam
using webcam = Microsoft.Robotics.Services.WebCam.Proxy;

// No longer required
//using cs = Microsoft.Dss.Services.Constructor;
// For locating services in the Directory
using ds = Microsoft.Dss.Services.Directory;

// For HttpGet
using Microsoft.Dss.Core.DsspHttp;
// For HttpStatusCode
using System.Net;
// Specifically for HttpPost handling
using System.Collections.ObjectModel;
using System.Collections.Specialized;
using Microsoft.Dss.Core.DsspHttpUtilities;

// For StringBuilder
//using System.Text;


namespace ProMRDS.Robotics.TeleOperation
{

    /// <summary>
    /// Implementation class for TeleOperation
    /// </summary>
    [DisplayName("TeleOperation")]
    [Description("TeleOperation Service using Generic Differential Drive and a WebCam")]
    [Contract(Contract.Identifier)]
    public class TeleOperationService : DsspServiceBase
    {
        private const string InitialStateUri = ServicePaths.MountPoint + @"/ProMRDS/Config/TeleOperation.Config.xml";

        /// <summary>
        /// TeleOperation State
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = InitialStateUri)]
        private TeleOperationState _state = new TeleOperationState();

        /// <summary>
        /// Embedded XSLT file for formatting State on a web page
        /// </summary>
        [EmbeddedResource("ProMRDS.Robotics.TeleOperation.Resources.TeleOperation.xslt")]
        string _transform = null;
        //string _transform = "/resources/teleoperation/ProMRDS.Robotics.TeleOperation.TeleOperation.xslt";

        [EmbeddedResource("ProMRDS.Robotics.TeleOperation.Resources.Fault.xslt")]
        string _faultTransform = null;

        // Required for Http Post handling
        // NOTE: Do NOT create a new port because it gives you the mistaken idea
        // that you can send messages to it when in fact there is no service to
        // respond to the messages! You must create the new service during the
        // initialization process -- see Start(). Leave it null and your code
        // will crash if you forget to create the service.
        //DsspHttpUtilitiesPort _httpUtilities = new DsspHttpUtilitiesPort();
        DsspHttpUtilitiesPort _httpUtilities;

        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/teleoperation", AllowMultipleInstances=false)]
        private TeleOperationOperations _mainPort = new TeleOperationOperations();

        // Use the local Game Controller -- this means DSS must be running locally
        [Partner("GameController", Contract = game.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        game.GameControllerOperations _gameControllerPort = new game.GameControllerOperations();
        game.GameControllerOperations _gameControllerNotify = new game.GameControllerOperations();
        Port<Shutdown> _gameControllerShutdown = new Port<Shutdown>();

        // Ports for controlling the Differential Drive
        drive.DriveOperations _drivePort = null;
        drive.DriveOperations _driveNotify = new drive.DriveOperations();
        Port<Shutdown> _driveShutdown = null;

        // Handle to the main WinForm UI
        DriveControl _driveControl;
        // Port for the UI to send messages back to here (main service)
        DriveControlEvents _eventsPort = new DriveControlEvents();

        // Ports for the Web Camera
        webcam.WebCamOperations _webCamPort;
        webcam.WebCamOperations _webCamNotify = new webcam.WebCamOperations();
        Port<Shutdown> _webCamShutdown = null;
        // Form to display the video in
        WebCamForm _cameraForm;
        // Port for the WebCam Form to communicate on
        WebCamFormEvents _webCamEventsPort = new WebCamFormEvents();
        // Flag to indicate that the form is loaded and ready
        bool _webCamFormLoaded = false;

        // This scale factor is applied to power settings which must be in the
        // range -1 to +1. However, the game controller and trackball have a
        // range of -1000 to +1000.
        double MOTOR_POWER_SCALE_FACTOR = 0.001;


        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public TeleOperationService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

            // Needed for HttpPost
            _httpUtilities = DsspHttpUtilitiesService.Create(Environment);

            base.Start();

            // Attempt to connect to a game controller straight away
            //SpawnIterator(SubscribeToGameController);

            // Hook up all of the Form events
            MainPortInterleave.CombineWith(Arbiter.Interleave(
                new TeardownReceiverGroup
                (
                ),
                new ExclusiveReceiverGroup
                (
                    // Form handling
                    Arbiter.ReceiveWithIterator<OnLoad>(true, _eventsPort, OnDriveControlLoadHandler),
                    Arbiter.Receive<OnClosed>(true, _eventsPort, OnDriveControlClosedHandler),
                    Arbiter.ReceiveWithIterator<OnLoad>(true, _webCamEventsPort, OnWebCamFormLoadHandler),
                    Arbiter.Receive<OnClosed>(true, _webCamEventsPort, OnWebCamFormClosedHandler),
                    // Connection request
                    Arbiter.ReceiveWithIterator<OnConnect>(true, _eventsPort, OnConnectHandler)
                ),
                new ConcurrentReceiverGroup
                (
                    // Game Controller - Added later
//                    Arbiter.ReceiveWithIterator<game.Replace>(true, _gameControllerNotify, JoystickReplaceHandler),
//                    Arbiter.ReceiveWithIterator<game.UpdateAxes>(true, _gameControllerNotify, JoystickUpdateAxesHandler),
//                    Arbiter.ReceiveWithIterator<game.UpdateButtons>(true, _gameControllerNotify, JoystickUpdateButtonsHandler),

                    // Drive notifications - Not currently used
//                    Arbiter.ReceiveWithIterator<drive.Update>(true, _driveNotify, OnDriveUpdateNotificationHandler),

                    // Movement commands
                    Arbiter.ReceiveWithIterator<OnMove>(true, _webCamEventsPort, OnMoveHandler),
                    Arbiter.ReceiveWithIterator<OnMove>(true, _eventsPort, OnMoveHandler),
                    Arbiter.ReceiveWithIterator<OnMotionCommand>(true, _eventsPort, OnMotionCommandHandler),
                    // WebCam -- Be careful because this can lock up if Exclusive
                    Arbiter.ReceiveWithIterator<webcam.UpdateFrame>(true, _webCamNotify, WebCamUpdateFrameHandler)
                )
            ));

            // Create the WinForm UI
            WinFormsServicePort.Post(new RunForm(CreateForm));

        }

        #region Initialization

        private void InitializeState()
        {
            // TT - Added code to create a default State if no
            // config file exists
            if (_state == null)
            {
                _state = new TeleOperationState();
                _state.Host = "";
                _state.Port = 0;
            }

            // This is tacky, but we need to set the default values
            // in case there is no existing config.xml file
            if (_state.Options == null)
            {
                // Create a new options bag
                _state.Options = new GUIOptions();

                // Set the options to reasonable values
                _state.Options.WindowStartX = 0;
                _state.Options.WindowStartY = 0;
                _state.Options.WebCamStartX = 300;
                _state.Options.WebCamStartY = 0;
                _state.Options.DeadZoneX = 100;
                _state.Options.DeadZoneY = 100;
                _state.Options.TranslateScaleFactor = 1.0;
                _state.Options.RotateScaleFactor = 0.5;
                _state.Options.DriveDistance = 300;
                // Speed is actually power and it ranges from 0 to 1000
                // which corresponds to the range of the game controller.
                // This is mapped back to 0 to 1.0 before being used.
                _state.Options.MotionSpeed = 300;
                // Angle is in degrees
                _state.Options.RotateAngle = 45;
                // Camera update interval in milliseconds
                // Note that this is only required for the
                // simulated webcam because it does not provide
                // updates when you subscribe
                //_state.Options.CameraInterval = 250;
            }
            //if (_state.Options.CameraInterval < 100)
            //    _state.Options.CameraInterval = 100;

            // Make sure that we don't propagate previous connections
            _state.Connected = false;
            _state.DriveService = "";
            _state.WebCamService = "";

            SaveState(_state);
        }


        #endregion

        #region WinForms interaction

        System.Windows.Forms.Form CreateForm()
        {
            // NOTE: Modify the constructor in the Form to pass these parameters
            return new DriveControl(_eventsPort, _state);
        }

        #endregion

        #region Operation Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            // Format the response using a transform
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }

        /// <summary>
        /// Http Post Handler for Web Form inputs
        /// </summary>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost httpPost)
        {
            string ErrorMessage = String.Empty;
            Fault fault = null;
            NameValueCollection parameters = new NameValueCollection();
            /*
            // Extract the parameters from the Request data stream
            int length = 0;
            byte[] data = new byte[10240];
            length = httpPost.Body.Context.Request.InputStream.Read(data, 0, 10240);
            StringBuilder sb = new StringBuilder();
            for (int i = 0; i < length; i++)
                sb.Append((char)data[i]);

            string request = sb.ToString();
            char[] ampersand = { '&' };
            char[] equals = { '=' };
            string[] pairs = request.Split(ampersand);
            for (int i = 0; i < pairs.Length; i++)
            {
                string[] namevalue = pairs[i].Split(equals);
                parameters.Add(namevalue[0], namevalue[1]);
            }

            Console.WriteLine(sb.ToString());

            if (length == 0)
                Console.WriteLine("No data: " + length);
            */

            // TT Jul-2008 - Changed for CTP of V2.0
            // Use helper to read form data
            //ReadFormData readForm = new ReadFormData(httpPost.Body.Context);
            ReadFormData readForm = new ReadFormData(httpPost);
            _httpUtilities.Post(readForm);

            // Wait for result
            yield return Arbiter.Choice(
                readForm.ResultPort,
                delegate(NameValueCollection col)
                {
                    parameters = col;
                },
                delegate(Exception e)
                {
                    fault = Fault.FromException(e);
                    LogError(null, "Error processing form data", fault);
                    ErrorMessage += e.Message;
                }
            );

            if (fault != null)
            {
                httpPost.ResponsePort.Post(fault);
                yield break;
            }

            // For debugging only ...
            /*
            Console.WriteLine("Form Parameters:");
            for (int i = 0; i < parameters.Count; i++)
            {
                Console.WriteLine(parameters[i]);
            }
             */

            double MotionSpeed = _state.Options.MotionSpeed;
            double DeadZoneX = _state.Options.DeadZoneX;
            double DeadZoneY = _state.Options.DeadZoneY;
            double TranslateScaleFactor = _state.Options.TranslateScaleFactor;
            double DriveDistance = _state.Options.DriveDistance;
            double RotateScaleFactor = _state.Options.RotateScaleFactor;
            double RotateAngle = _state.Options.RotateAngle;
            bool validValue = false;

            if (!string.IsNullOrEmpty(parameters["MotionSpeed"]))
            {
                try
                {
                    MotionSpeed = double.Parse(parameters["MotionSpeed"]);
                    validValue = true;
                }
                catch (Exception e)
                {
                    string msg = "Could not parse Motion Speed: " + e.Message;
                    LogError(msg);
                    ErrorMessage += msg;
                }
            }

            if (validValue && MotionSpeed >= 0 && MotionSpeed <= 1000)
            {
                _state.Options.MotionSpeed = MotionSpeed;
            }

            validValue = false;
            if (!string.IsNullOrEmpty(parameters["DeadZoneX"]))
            {
                try
                {
                    DeadZoneX = double.Parse(parameters["DeadZoneX"]);
                    validValue = true;
                }
                catch (Exception e)
                {
                    string msg = "Could not parse Dead Zone X: " + e.Message;
                    LogError(msg);
                    ErrorMessage += msg;
                }
                if (validValue)
                    _state.Options.DeadZoneX = DeadZoneX;
            }

            validValue = false;
            if (!string.IsNullOrEmpty(parameters["DeadZoneY"]))
            {
                try
                {
                    DeadZoneY = double.Parse(parameters["DeadZoneY"]);
                    validValue = true;
                }
                catch (Exception e)
                {
                    string msg = "Could not parse Dead Zone Y: " + e.Message;
                    LogError(msg);
                    ErrorMessage += msg;
                }
                if (validValue)
                    _state.Options.DeadZoneY = DeadZoneY;
            }

            validValue = false;
            if (!string.IsNullOrEmpty(parameters["TranslateScaleFactor"]))
            {
                try
                {
                    TranslateScaleFactor = double.Parse(parameters["TranslateScaleFactor"]);
                    validValue = true;
                }
                catch (Exception e)
                {
                    string msg = "Could not parse Translate Scale Factor: " + e.Message;
                    LogError(msg);
                    ErrorMessage += msg;
                }
                if (validValue)
                    _state.Options.TranslateScaleFactor = TranslateScaleFactor;
            }

            validValue = false;
            if (!string.IsNullOrEmpty(parameters["RotateScaleFactor"]))
            {
                try
                {
                    RotateScaleFactor = double.Parse(parameters["RotateScaleFactor"]);
                    validValue = true;
                }
                catch (Exception e)
                {
                    string msg = "Could not parse Rotate Scale Factor: " + e.Message;
                    LogError(msg);
                    ErrorMessage += msg;
                }
                if (validValue)
                    _state.Options.RotateScaleFactor = RotateScaleFactor;
            }

            validValue = false;
            if (!string.IsNullOrEmpty(parameters["DriveDistance"]))
            {
                try
                {
                    DriveDistance = double.Parse(parameters["DriveDistance"]);
                    validValue = true;
                }
                catch (Exception e)
                {
                    string msg = "Could not parse Drive Distance: " + e.Message;
                    LogError(msg);
                    ErrorMessage += msg;
                }
                if (validValue)
                    _state.Options.DriveDistance = DriveDistance;
            }

            validValue = false;
            if (!string.IsNullOrEmpty(parameters["RotateAngle"]))
            {
                try
                {
                    RotateAngle = double.Parse(parameters["RotateAngle"]);
                    validValue = true;
                }
                catch (Exception e)
                {
                    string msg = "Could not parse Rotate Angle: " + e.Message;
                    LogError(msg);
                    ErrorMessage += msg;
                }
                if (validValue)
                    _state.Options.RotateAngle = RotateAngle;
            }


            // Finally, process the result
            if (ErrorMessage == string.Empty)
            {
                HttpPostSuccess(httpPost);
            }
            else
            {
                HttpPostFailure(httpPost, ErrorMessage);
            }

            yield break;
        }

        /// <summary>
        /// Send Http Post Success Response
        /// </summary>
        private void HttpPostSuccess(HttpPost httpPost)
        {
            // Grab the current window location
            if (_driveControl != null)
            {
                _state.Options.WindowStartX = _driveControl.Location.X;
                _state.Options.WindowStartY = _driveControl.Location.Y;
            }
            if (_cameraForm != null)
            {
                _state.Options.WebCamStartX = _cameraForm.Location.X;
                _state.Options.WebCamStartY = _cameraForm.Location.Y;
            }

            // Update the Forms with the new option settings
            // This is another implicit method of communication with WinForms
            if (_driveControl != null)
                _driveControl.options = _state.Options;
            if (_cameraForm != null)
                _cameraForm.options = _state.Options;

            // Post a response
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.OK, _state, _transform);
            httpPost.ResponsePort.Post(rsp);

            // Save the state now because it might have changed
            SaveState(_state);
        }

        /// <summary>
        /// Send Http Post Failure Response
        /// </summary>
        private void HttpPostFailure(HttpPost httpPost, string failureReason)
        {
            // Create a new Fault based on the error message
            Fault fault = Fault.FromCodeSubcodeReason(FaultCodes.Receiver, DsspFaultCodes.OperationFailed, failureReason);
            //HttpResponseType rsp = new HttpResponseType(fault);
            //httpPost.ResponsePort.Post(rsp);

            // Post it back but use the Fault Transform
            httpPost.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                fault,
                _faultTransform
                )
            );
        }

        /// <summary>
        /// Drop Handler shuts down
        /// </summary>
        /// <param name="drop"></param>
        [ServiceHandler(ServiceHandlerBehavior.Teardown)]
        public void DropHandler(DsspDefaultDrop drop)
        {
            SpawnIterator(drop, DropIterator);
        }

        // It is very important to shut everything down cleanly
        // so that the DSS node will exit
        IEnumerator<ITask> DropIterator(DsspDefaultDrop drop)
        {
            LogInfo("Starting Drop");

            // Close the WebCam Form
            if (_cameraForm != null)
            {
                WebCamForm cam = _cameraForm;
                _webCamFormLoaded = false;
                _cameraForm = null;

                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        if (!cam.IsDisposed)
                        {
                            cam.Dispose();
                        }
                    }
                );
            }

            // Close the Dashboard Form
            if (_driveControl != null)
            {
                DriveControl drive = _driveControl;
                _driveControl = null;

                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        if (!drive.IsDisposed)
                        {
                            drive.Dispose();
                        }
                    }
                );
            }

            if (_driveShutdown != null)
            {
                yield return PerformShutdown(ref _driveShutdown);
            }

            if (_webCamShutdown != null)
            {
                yield return PerformShutdown(ref _webCamShutdown);
            }

            // Now drop ourselves
            base.DefaultDropHandler(drop);

            // Sometimes this gets an exception because the service has already
            // been dropped and then the node does not shut down
            if (ControlPanelPort != null)
                ControlPanelPort.Shutdown();

        }

        Choice PerformShutdown(ref Port<Shutdown> port)
        {
            Shutdown shutdown = new Shutdown();
            port.Post(shutdown);
            port = null;

            return Arbiter.Choice(
                shutdown.ResultPort,
                delegate(SuccessResult success) { },
                delegate(Exception e)
                {
                    LogError(e);
                }
            );
        }

        #endregion

        #region Game Controller Handlers

        private IEnumerator<ITask> SubscribeToGameController()
        {
            bool success = false;

            LogInfo("Subscribing to Game Controller");

            // Create a subscription message to subscribe to the Game Controller service
            game.Subscribe msg = new game.Subscribe();
            msg.NotificationPort = _gameControllerNotify;
            // Specify a Shutdown port so we can unsubscribe later
            msg.NotificationShutdownPort = _gameControllerShutdown;
            // Post the message
            _gameControllerPort.Post(msg);
            // Wait for a response
            yield return Arbiter.Choice(
                msg.ResponsePort,
                delegate(SubscribeResponseType response) { success = true; },
                delegate(Fault fault) { LogError(fault); success = false; }
            );

            if (!success)
                yield break;    // Subscription failed

            // Add receivers to the main interleave for each of the possible
            // notification messages from the game controller.
            // If there are no game controllers, then there will be no messages!
            MainPortInterleave.CombineWith(new Interleave(
                new ExclusiveReceiverGroup(),
                new ConcurrentReceiverGroup
                (
                    Arbiter.ReceiveWithIterator<game.Replace>(true, _gameControllerNotify, GameReplaceHandler),
                    Arbiter.ReceiveWithIterator<game.UpdateAxes>(true, _gameControllerNotify, GameUpdateAxesHandler),
                    Arbiter.ReceiveWithIterator<game.UpdateButtons>(true, _gameControllerNotify, GameUpdateButtonsHandler)
                )
            ));

            LogInfo("Game Controller subscription successful");

        }

        // A Game Controller can issue three types of messages when you subscribe:
        // Replace
        // Axes
        // Buttons
        // These are the handlers for each type of message.
        // Notice that they all pass their messages across to the WinForm.

        IEnumerator<ITask> GameReplaceHandler(game.Replace replace)
        {
            // The patter here is similar for all three handlers
            if (_driveControl != null)
            {
                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        _driveControl.UpdateGameControllerButtons(replace.Body.Buttons);
                        _driveControl.UpdateGameControllerAxes(replace.Body.Axes);
                    }
                );
            }

            yield break;
        }

        IEnumerator<ITask> GameUpdateAxesHandler(game.UpdateAxes update)
        {
            // Check that the DriveControl Form is active
            if (_driveControl != null)
            {
                // Send the update info to the Game Controller code in the
                // DriveControl Form by executing a method inside the Form
                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        _driveControl.UpdateGameControllerAxes(update.Body);
                    }
                );
            }
            yield break;
        }

        IEnumerator<ITask> GameUpdateButtonsHandler(game.UpdateButtons update)
        {
            if (_driveControl != null)
            {
                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        _driveControl.UpdateGameControllerButtons(update.Body);
                    }
                );
            }
            yield break;
        }

        #endregion

        #region Drive Control Event Handlers

        /// <summary>
        /// On Load Handler for completion of Drive Control Form Load
        /// </summary>
        /// <param name="onLoad"></param>
        /// <returns></returns>
        IEnumerator<ITask> OnDriveControlLoadHandler(OnLoad onLoad)
        {
            // Save a handle to the form
            _driveControl = (DriveControl)onLoad.Form;

            LogInfo("Drive Control Form Loaded");

            // Find a joystick if possible
            //yield return EnumerateGameControllers();
            // Subscribe to the joystick once the form has loaded
            yield return Arbiter.ExecuteToCompletion(Environment.TaskQueue,
                Arbiter.FromIteratorHandler(SubscribeToGameController));
        }

        /// <summary>
        /// Form Closed Handler for when Drive Control Form has closed
        /// </summary>
        /// <param name="onClosed"></param>
        void OnDriveControlClosedHandler(OnClosed onClosed)
        {
            if (onClosed.Form == _driveControl)
            {
                LogInfo("Main Form Closed");

                // Send a Drop message to ourselves
                _mainPort.Post(new DsspDefaultDrop(DropRequestType.Instance));
            }
        }

        /// <summary>
        /// Connect Handler
        /// </summary>
        /// <param name="onConnect"></param>
        /// <returns></returns>
        IEnumerator<ITask> OnConnectHandler(OnConnect onConnect)
        {
            if (onConnect.Form == _driveControl)
            {
                string ErrorMessage = null;

                // The service here is the Directory on the specified host:port
                UriBuilder builder = new UriBuilder(onConnect.Service);
                builder.Scheme = new Uri(ServiceInfo.Service).Scheme;

                ds.DirectoryPort port = ServiceForwarder<ds.DirectoryPort>(builder.Uri);
                ds.Get get = new ds.Get();
                // Set a timeout just in case
                get.TimeSpan = TimeSpan.FromMilliseconds(2000);

                port.Post(get);
                ServiceInfoType[] list = null;

                yield return Arbiter.Choice(get.ResponsePort,
                    delegate(ds.GetResponseType response)
                    {
                        list = response.RecordList;
                    },
                    delegate(Fault fault)
                    {
                        list = new ServiceInfoType[0];
                        LogError(fault);
                    }
                );

                ServiceInfoType driveInfo = null;
                ServiceInfoType webcamInfo = null;

                try
                {
                    if (list.Length > 0)
                    {
                        UriBuilder node = new UriBuilder(list[0].Service);
                        node.Path = null;
                        string nodestring = node.Host + ":" + node.Port;
                        LogInfo(nodestring);
                    }
                    else
                    {
                        LogError("No services found!");
                        ErrorMessage = "No services found!\n";
                        _state.Connected = false;
                    }

                    string driveUriPath = null;
                    string webcamUriPath = null;
                    foreach (ServiceInfoType info in list)
                    {
                        if (driveInfo == null && info.Contract == drive.Contract.Identifier)
                        {
                            driveInfo = info;
                        }

                        if (webcamInfo == null && info.Contract == webcam.Contract.Identifier)
                        {
                            webcamInfo = info;
                        }

                        if (driveInfo != null && webcamInfo != null)
                            break;
                    }
                }
                catch (Exception ex)
                {
                    string msg = "Service search error: " + ex.Message; 
                    LogError(msg);
                    ErrorMessage += msg + "\n";
                    _state.Connected = false;
                }

                if (driveInfo == null)
                    ErrorMessage += "No Drive service found\n";
                if (webcamInfo == null)
                    ErrorMessage += "No WebCam service found\n";

                if (ErrorMessage != null)
                {
                    if (_driveControl != null)
                    {
                        WinFormsServicePort.FormInvoke(
                            delegate()
                            {
                                _driveControl.ShowErrorMessage(ErrorMessage);
                            }
                        );
                    }
                }

                if (driveInfo != null)
                    SpawnIterator<string>(driveInfo.Service, ConnectDrive);

                if (webcamInfo != null)
                    SpawnIterator<string>(webcamInfo.Service, ConnectWebCam);

                // We are "connected" if either service was found
                if (driveInfo != null || webcamInfo != null)
                    _state.Connected = true;
            }
        }

        #endregion

        #region Drive Operations

        /// <summary>
        /// Connect to a Generic Differential Drive service
        /// </summary>
        /// <param name="service"></param>
        /// <returns></returns>
        IEnumerator<ITask> ConnectDrive(string service)
        {
            drive.EnableDriveRequest request = new drive.EnableDriveRequest();

            // Already connected?
            if (_drivePort != null)
            {
                // Disable drive
                yield return Arbiter.Choice(
                    _drivePort.EnableDrive(request),
                    delegate(DefaultUpdateResponseType response) { },
                    delegate(Fault f)
                    {
                        LogError(f);
                    }
                );

                // Unsubscribe
                if (_driveShutdown != null)
                {
                    if (_driveShutdown != null)
                        yield return PerformShutdown(ref _driveShutdown);
                }
            }

            // Get a new drive port
            _drivePort = ServiceForwarder<drive.DriveOperations>(service);

            request.Enable = true;

            // Enable the drive
            yield return Arbiter.Choice(
                _drivePort.EnableDrive(request),
                delegate(DefaultUpdateResponseType response) { },
                delegate(Fault f)
                {
                    LogError(f);
                }
            );

            // Record the service URI so the user can see it
            _state.DriveService = service;
            LogInfo("Connected Drive to " + service);

            /*
            // Subscribe to the drive
            _driveShutdown = new Port<Shutdown>();
            drive.ReliableSubscribe subscribe = new drive.ReliableSubscribe(
                new ReliableSubscribeRequestType(10)
            );
            subscribe.NotificationPort = _driveNotify;
            subscribe.NotificationShutdownPort = _driveShutdown;

            _drivePort.Post(subscribe);

            yield return Arbiter.Choice(
                subscribe.ResponsePort,
                delegate(SubscribeResponseType response)
                {
                    LogInfo("Subscribed to " + service);
                },
                delegate(Fault fault)
                {
                    _driveShutdown = null;
                    LogError(fault);
                }
            );
             */
        }

        /*
        // Handler for Drive Notifications
        IEnumerator<ITask> OnDriveUpdateNotificationHandler(drive.Update notification)
        {
            if (_driveControl != null)
            {
                // Display info about the motors
                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        _driveControl.UpdateMotorData(notification.Body);
                    }
                );
            }

            LogObject(notification.Body);
            yield break;
        }
        */

        /// <summary>
        /// Handle basic moves (motor power settings)
        /// </summary>
        /// <param name="onMove"></param>
        /// <returns></returns>
        IEnumerator<ITask> OnMoveHandler(OnMove onMove)
        {
            Port<OnMove> p = (Port<OnMove>)_eventsPort[typeof(OnMove)];
            if (p.ItemCount > 10)
                Console.WriteLine("OnMove backlog: " + p.ItemCount);

            if (_drivePort != null)
            {
                // Create a drive request
                // There is a more concise syntax, but we need to access some
                // additional properties
                drive.SetDrivePowerRequest request = new drive.SetDrivePowerRequest();
                request.LeftWheelPower = (double)onMove.Left * MOTOR_POWER_SCALE_FACTOR;
                request.RightWheelPower = (double)onMove.Right * MOTOR_POWER_SCALE_FACTOR;
                drive.SetDrivePower sdp = new drive.SetDrivePower(request);
                // Set a timeout so that this does not wait forever
                sdp.TimeSpan = TimeSpan.FromMilliseconds(500);
                _drivePort.Post(sdp);

                yield return Arbiter.Choice(
                        sdp.ResponsePort,
                        delegate(DefaultUpdateResponseType response)
                        {
                            //Console.WriteLine("Power updated");
                        },
                        delegate(Fault f)
                        {
                            // Log an error (most probably a timeout)
                            LogError(f);
                            if (f.Code.Subcode.Value == DsspFaultCodes.ResponseTimeout)
                                Console.WriteLine("Timeout on Move");
                            else
                                Console.WriteLine(f.Detail);
                        }
                );
            }
        }

        /// <summary>
        /// Handle motions that require a parameter
        /// </summary>
        /// <param name="onCommand"></param>
        /// <returns></returns>
        IEnumerator<ITask> OnMotionCommandHandler(OnMotionCommand onCommand)
        {
            if (onCommand.Form == _driveControl && _drivePort != null)
            {
                // NOTE: We can't set a timeout on these motions because we don't
                // know how long they will take
                switch (onCommand.Command)
                {
                    case MOTION_COMMANDS.Rotate:
                        drive.RotateDegreesRequest rotRequest = new drive.RotateDegreesRequest();
                        rotRequest.Degrees = onCommand.Parameter;
                        rotRequest.Power = (double)onCommand.Power * MOTOR_POWER_SCALE_FACTOR;

                        yield return Arbiter.Choice(
                            _drivePort.RotateDegrees(rotRequest),
                            delegate(DefaultUpdateResponseType response) { },
                            delegate(Fault f)
                            {
                                LogError(f);
                            }
                        );
                        break;

                    case MOTION_COMMANDS.Translate:
                        drive.DriveDistanceRequest transRequest = new drive.DriveDistanceRequest();
                        transRequest.Distance = onCommand.Parameter;
                        transRequest.Power = (double)onCommand.Power * MOTOR_POWER_SCALE_FACTOR;

                        yield return Arbiter.Choice(
                            _drivePort.DriveDistance(transRequest),
                            delegate(DefaultUpdateResponseType response) { },
                            delegate(Fault f)
                            {
                                LogError(f);
                            }
                        );
                        break;

                    case MOTION_COMMANDS.Stop:
                    default:
                        // Changed for RDS 2008 because AllStop disables the drive
                        LogInfo("Stop");
                        _drivePort.SetDrivePower(0, 0);

                        /*
                        LogInfo("Requesting EStop");
                        drive.AllStopRequest stopRequest = new drive.AllStopRequest();

                        yield return Arbiter.Choice(
                            _drivePort.AllStop(stopRequest),
                            delegate(DefaultUpdateResponseType response) { },
                            delegate(Fault f)
                            {
                                LogError(f);
                            }
                        );
                         */
                        break;
                }
            }
            yield break;
        }

        #endregion

        #region Camera

        // Create a form for the webcam
        // Called when a new connection is made
        Form CreateWebCamForm()
        {
            if (_cameraForm == null)
                _cameraForm = new WebCamForm(_webCamEventsPort, _state);
            return _cameraForm;
        }

        /// <summary>
        /// On Load Handler for completion of WebCam Form Load
        /// </summary>
        /// <param name="onWebCamFormLoad"></param>
        /// <returns></returns>
        IEnumerator<ITask> OnWebCamFormLoadHandler(OnLoad onLoad)
        {
            // Save a handle to the form
            //_cameraForm = onLoad.WebCamForm;

            LogInfo("WebCam Form Loaded");

            _webCamFormLoaded = true;

            yield break;
        }


        /// <summary>
        /// Form Closed Handler for when WebCam Form has closed
        /// </summary>
        /// <param name="onWebCamFormClosed"></param>
        void OnWebCamFormClosedHandler(OnClosed onClosed)
        {
            if (onClosed.Form == _cameraForm)
            {
                LogInfo("WebCam Form Closed");
                _webCamFormLoaded = false;
                _cameraForm = null;
            }
        }


        // Handler for connecting to WebCam
        IEnumerator<ITask> ConnectWebCam(string camera)
        {
            //ServiceInfoType info = null;
            Fault fault = null;
            SubscribeResponseType s;
            //String camera = Opt.Service;

            // Already connected?
            if (_webCamPort != null)
            {
                // Unsubscribe
                if (_webCamShutdown != null)
                    yield return PerformShutdown(ref _webCamShutdown);
            }

            // Create a new port
            _webCamPort = ServiceForwarder<webcam.WebCamOperations>(camera);

            // Subscribe to the webcam
            webcam.Subscribe subscribe = new webcam.Subscribe();
            subscribe.NotificationPort = _webCamNotify;
            subscribe.NotificationShutdownPort = _webCamShutdown;

            _webCamPort.Post(subscribe);

            yield return Arbiter.Choice(
                //_webCamPort.Subscribe(_webCamNotify),
                subscribe.ResponsePort,
                delegate(SubscribeResponseType success)
                { s = success; },
                delegate(Fault f)
                {
                    fault = f;
                }
            );

            if (fault != null)
            {
                LogError(null, "Failed to subscribe to webcam", fault);
                yield break;
            }

            // Put the service URI into the state for visibility
            _state.WebCamService = camera;
            LogInfo("Connected WebCam to " + camera);

            // Now that we have found the service and subscribed,
            // create a form to display the video
            RunForm runForm = new RunForm(CreateWebCamForm);

            WinFormsServicePort.Post(runForm);

            yield return Arbiter.Choice(
                runForm.pResult,
                delegate(SuccessResult success) { },
                delegate(Exception e)
                {
                    fault = Fault.FromException(e);
                }
            );

            if (fault != null)
            {
                LogError(null, "Failed to Create WebCam window", fault);
                yield break;
            }
            // The following code falls over with a null pointer
            // exception inside MainPortInterleave.CombineWith.
            // Since I don't know why, I have added the message
            // to the interleave that is created at startup.
            // This is a bit tacky, but does not seem to do any harm.
            /*
            Interleave x = Arbiter.Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(
                        Arbiter.ReceiveWithIterator<cam.UpdateFrame>(true, _webCamNotify, WebCamUpdateFrameHandler)
                    ),
                    new ConcurrentReceiverGroup()
            );

            base.MainPortInterleave.CombineWith(x);
            */

            // There is a bug in the simulated webcam. It does not
            // automatically send UpdateFrame messages when you
            // subscribe. If this is a simulated camera, then set
            // up a timer to poke the webcam service and make it
            // send a frame.
            /*
            if (camera.ToLower().Contains("simul"))
            {
                Activate(
                    Arbiter.Receive(false, TimeoutPort(_state.Options.CameraInterval), CameraTimer)
                );
            }
            */
            yield break;

        }

        // Timer for simulated cameras ONLY
        /*
        void CameraTimer(DateTime signal)
        {
            //LogInfo(LogGroups.Console, "Send UpdateFrame to webcam");
            webcam.UpdateFrame request = new webcam.UpdateFrame();
            _webCamPort.Post(request);
            Activate(
                Arbiter.Receive(false, TimeoutPort(_state.Options.CameraInterval), CameraTimer)
            );
        }
        */

        // Handler for new frames from the camera
        IEnumerator<ITask> WebCamUpdateFrameHandler(webcam.UpdateFrame update)
        {
            webcam.QueryFrameResponse frame = null;
            Fault fault = null;

            // Don't do anything if the form has not loaded or has been closed!
            // Race conditions can arise when the form is first created, or if
            // the user closes the form. These result in access violations unless
            // we are careful not to execute the rest of the code.
            if (!_webCamFormLoaded)
                yield break;

            // Throw away the backlog
            // This does no harm because we are throwing away notifications,
            // not webcam images
            Port<webcam.UpdateFrame> p = (Port<webcam.UpdateFrame>)_webCamNotify[typeof(webcam.UpdateFrame)];
            if (p.ItemCount > 2)
            {
                Console.WriteLine("Webcam backlog: " + p.ItemCount);
                p.Clear();
            }

            webcam.QueryFrame query = new webcam.QueryFrame();
            // Set a timeout so that this cannot wait forever
            query.TimeSpan = TimeSpan.FromMilliseconds(1000);
            _webCamPort.Post(query);

            // Wait for response
            yield return Arbiter.Choice(
                query.ResponsePort,
                delegate(webcam.QueryFrameResponse success)
                {
                    frame = success;
                },
                delegate(Fault f)
                {
                    fault = f;
                }
            );

            if (fault != null)
            {
                LogError(null, "Failed to get frame from camera", fault);
                yield break;
            }

            // Create a bitmap from the webcam response and display it
            Bitmap bmp = MakeBitmap(frame.Size.Width, frame.Size.Height, frame.Frame);
            // Display the image in the WinForm
            SpawnIterator<Bitmap>(bmp, DisplayImage);

            yield break;
        }


        Bitmap MakeBitmap(int width, int height, byte[] imageData)
        {
            // NOTE: This code implicitly assumes that the width is a multiple
            // of four bytes because Bitmaps have to be longword aligned.
            // We really should look at bmp.Stride to see if there is any padding.
            // However, the width and height come from the webcam and most cameras
            // have resolutions that are multiples of four.

            Bitmap bmp = new Bitmap(width, height, PixelFormat.Format24bppRgb);

            BitmapData data = bmp.LockBits(
                new Rectangle(0, 0, bmp.Width, bmp.Height),
                ImageLockMode.WriteOnly,
                PixelFormat.Format24bppRgb
            );

            Marshal.Copy(imageData, 0, data.Scan0, imageData.Length);

            bmp.UnlockBits(data);

            return bmp;
        }


        // Display an image in the WebCam Form
        IEnumerator<ITask> DisplayImage(Bitmap bmp)
        {
            Fault fault = null;

            // Insurance in case the form was closed
            if (!_webCamFormLoaded)
                yield break;

            FormInvoke setImage = new FormInvoke(
                delegate()
                {
                    if (_webCamFormLoaded)
                        _cameraForm.CameraImage = bmp;
                }
            );

            WinFormsServicePort.Post(setImage);

            yield return Arbiter.Choice(
                setImage.ResultPort,
                delegate(EmptyValue success) { },
                delegate(Exception e)
                {
                    fault = Fault.FromException(e);
                }
            );

            if (fault != null)
            {
                LogError(null, "Unable to set camera image on form", fault);
            }
            else
            {
                // LogInfo("New camera frame");
            }
            yield break;
        }

        #endregion

    }
}
